/**
 ****************************************************************************************************
 * @file        foc_app.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       FOC对外提供调用的接口
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */


#include "foc_app.h"
#include <stdint.h>

#if 1
/*酷飞2207 kv1960*/

#define L   (6.05f/1000000.0f)      /* 6.05uH */
#define R   (68.0f/1000.0f)       /* 68 mΩ */
/* 磁通: 0.600 mWb */
#define Tc  L/R/10.0f

/*平稳的PID参数*/
#define Kp_q  L/Tc/1000/5*2         
#define Ki_q  R/Tc/1000000/5*2*3

#define Kp_d       0.0533333309f
#define Ki_d       0.0519480482f

#define Kp_s    0.9375f         
#define Ki_s    0.000464062527f


/*激进的PID参数*/
// #define Kp_q  L*1000.0f
// #define Ki_q  R

// #define Kp_d       Kp_q*10
// #define Ki_d       Ki_q*10
// #define Kp_s    0.025f         
// #define Ki_s    0.464062527f

#define PID_QD_OUTLIMIT         20.0f              /*qd轴电流环 输出值限制*/
#define PID_QD_OUTLIMIT_HFI     0.7f               /*qd轴电流环 输出值限制-高频状态  该电机Uq 0.7V 空载大概2500r/min*/

FOC_MOTOR_t FOC_MOTOR = {
    .target_speed = 15000,                          /*目标速度*/
    .pole_pairs = 7,                                /*极对数*/
    .power = 24.0f,                                 /*供电电压*/

    /*电流输出限制*/
    .pid_qd_limit = PID_QD_OUTLIMIT,
    .pid_qd_limit_hif = PID_QD_OUTLIMIT_HFI,

    /*Q轴电流环 PID*/
    .pid_cur_iq ={
        .P = Kp_q,
        .I = Ki_q,
        .D = 0.0f,
      
        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,                   
        .ActualValue_limit = 20.0f,    
    },

    /*D轴电流环 PID*/
    .pid_cur_id ={
        .P = Kp_d,
        .I = Ki_d,
        .D = 0.0f,

        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,
        .ActualValue_limit = 20.0f,      
    },

    /*速度环 PID*/
    .pid_speed ={
        .P = Kp_s,        
        .I = Ki_s,
        
        .D = 0.000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 50.0f,
        .change_limit = 15.0f,    
    },
    .pid_speed_pos ={
        .P = 0.001f,     /*位置式速度PID参数*/
        .I = 0.007f,
        .D = 0.0000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 15.0f,
        .change_limit = 0.5f,    
    },

    /*电流设置*/
    .current = {
        .over = 20.0f,
    },

    /*无感设置*/
    .sensorless ={
        .hfi_freq = FOC_HFI_FREQ_10K,               /*高频注入频率*/
        .hfi_vol_ref = 0.5f,                        /*高频注入电压*/     
        .d_bias_vol = 0.1f,                         /*d轴偏置电压*/

        .vf.k = 0.0518748574f,                      /*vf强拖系数*/

        .up_smo = 2000.0f,                          /*切入滑模的速度*/
        .smo_down = 1800.0f,                        /*切出滑模的速度*/
  
        .smo = {                                    /*滑模系数*/
            .h = 0.5f,
            .pll = {
                .Kp = 2700.0f,
                .Ki = 364975.0f,
            },    
        },
        .hfi ={                                     /*高频注入系数*/
            .pll = {
                .Kp = 135.0f,
                .Ki = 9.23320198f,
            },
        },
    },
};

#endif




#if 0
/*原子PMSM电机*/

#define L   (0.58466f/1000.0f)       /* 584uH */
#define R   (1.096f)                /* 1.96Ω */
/* 磁通: 0.600 mWb */
#define Tc  L/R/10.0f

#define Kp_q  0.00533333309f /3    
#define Ki_q  0.0519480482f /3

#define Kp_d       0.533333309f
#define Ki_d       0.519480482f

#define Kp_s    0.46875f         
#define Ki_s    0.000464062527f

#define PID_QD_OUTLIMIT         8.0f               /*qd轴电流环 输出值限制*/
#define PID_QD_OUTLIMIT_HFI     4.0f               /*qd轴电流环 输出值限制-高频状态  该电机Uq 0.7V 空载大概2500r/min*/

FOC_MOTOR_t FOC_MOTOR = {
    .target_speed = 2000,                          /*目标速度*/
    .pole_pairs = 4,                                /*极对数*/
    .power = 24.0f,                                 /*供电电压*/

    /*电流输出限制*/
    .pid_qd_limit = PID_QD_OUTLIMIT,
    .pid_qd_limit_hif = PID_QD_OUTLIMIT_HFI,

    /*Q轴电流环 PID*/
    .pid_cur_iq ={
        .P = Kp_q,
        .I = Ki_q,
        .D = 0.0f,
      
        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,         /*根据电流传感器量程来选择*/
        .ActualValue_limit = 8.0f,    /*对应 3000r/min*/ 
    },

    /*D轴电流环 PID*/
    .pid_cur_id ={
        .P = Kp_d,
        .I = Ki_d,
        .D = 0.0f,

        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,
        .ActualValue_limit = 8.0f,      
    },

    /*速度环 PID*/
    .pid_speed ={
        .P = Kp_s,        
        .I = Ki_s,
        
        .D = 0.000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 50.0f,
        .change_limit = 15.0f,    
    },
    .pid_speed_pos ={
        .P = 0.001f,     /*位置式速度PID参数*/
        .I = 0.007f,
        .D = 0.0000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 15.0f,
        .change_limit = 0.5f,    
    },

    /*电流设置*/
    .current = {
        .over = 20.0f,
    },

    /*无感设置*/
    .sensorless ={
        .hfi_freq = FOC_HFI_FREQ_10K,                /*高频注入频率  这个电机电感大，要降低注入频率*/
        .hfi_vol_ref = 1.0f,                        /*高频注入电压*/     
        .d_bias_vol = 0.3f,                         /*d轴偏置电压*/

        .vf.k = 0.008f,                             /*vf强拖系数*/

        .up_smo = 600.0f,                          /*切入滑模的速度*/
        .smo_down = 500.0f,                        /*切出滑模的速度*/
  
        .smo = {                                    /*滑模系数*/
            .h = 0.1f,
            .pll = {
                .Kp = 270.0f,
                .Ki = 36497.0f,
            },    
        },
        .hfi ={                                     /*高频注入系数*/
            .pll = {
                .Kp = 135.0f,
                .Ki = 9.23320198f,
            },
        },
        .enc = {
            .PhaseShift = -0.59690237f,
        }
    },
    .hall.PhaseShift = 0.8f,                        /*霍尔偏移*/
};
#endif

#if 0
/*酷飞*/
/*酷飞2812 900KV电机


电机参数:
R:111.80mΩ          78.3333mΩ 线电阻          这里手动测得数据和驱动器测得数据不一致，使用手动得
L：8.44uH           12.53333uH 线电感
Lq-Ld：1.89uH
磁通：0.962mWb

*/
            
// #define L   0.00000844f
// #define R   0.11180f
#define L   0.00001253333f
#define R   0.0783333f
#define Tc  L/R/10.0f

#define Kp_q  L/Tc/1000/5*2         
#define Ki_q  R/Tc/1000000/5*2*3

#define Kp_d       0.0533333309f
#define Ki_d       0.0519480482f

#define Kp_s    0.9375f         
#define Ki_s    0.000464062527f

#define PID_QD_OUTLIMIT         20.0f              /*qd轴电流环 输出值限制*/
#define PID_QD_OUTLIMIT_HFI     0.7f               /*qd轴电流环 输出值限制-高频状态  该电机Uq 0.7V 空载大概1000r/min*/

FOC_MOTOR_t FOC_MOTOR = {
    .target_speed = 1000,
    .pole_pairs = 7,
    .power = 24.0f,

    .pid_qd_limit = PID_QD_OUTLIMIT,
    .pid_qd_limit_hif = PID_QD_OUTLIMIT_HFI,

    .pid_cur_iq ={
        .P = Kp_q,
        .I = Ki_q,
        .D = 0.0f,
      
        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,         /*根据电流传感器量程来选择*/
        .ActualValue_limit = 20.0f,    /*对应 3000r/min*/ 
    },

    .pid_cur_id ={
        .P = Kp_d,
        .I = Ki_d,
        .D = 0.0f,

        .SumError_limit = 50000000000.0f,
        .SetPoint_limit = 100.0f,
        .ActualValue_limit = 20.0f,      
    },

    .pid_speed ={
        .P = Kp_s,        /*速度在500以下时的pid参数*/
        .I = Ki_s,
        // .I = 464.062531f,
        
        .D = 0.000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 50.0f,
        .change_limit = 15.0f,    
    },
    .pid_speed_pos ={
        .P = 0.001f,     /*位置式速度PID参数*/
        .I = 0.007f,
        .D = 0.0000f,
  
        .SetPoint = 0.0f,

        .SumError_limit = 10000000.0f,
        .SetPoint_limit = 31000.0f,      /*最大转速范围*/
        .ActualValue_limit = 15.0f,
        .change_limit = 0.5f,    
    },

    .current = {
        .over = 20.0f,
    },

    .sensorless ={
        .hfi_freq = FOC_HFI_FREQ_5K,
        .hfi_vol_ref = 0.5f,
        .d_bias_vol = 0.1f,

        .vf.k = 0.0518748574f,

        .up_smo = 1200.0f,
        .smo_down = 1000.0f,    
  
        .smo = {
            .h = 0.5f,
            .pll = {
                .Kp = 540.0f,
                .Ki = 72995.0f,
                // .Kp = 135.0f,
                // .Ki = 36497.0f,
            },    
        },
        .hfi ={
            .pll = {
                // .Kp = 521.0f,
                // .Ki = 40000.0f,
                .Kp = 135.0f,
                // .Ki = 36497.0f,
                .Ki = 9.23320198f,
            },
        },
    },
};
#endif


void FOC_Init(void)
{
    /*定时器周期值*/
    uint32_t htim1_period = 2*(htim1.Init.Period+1);/*因为定时器1是中央对齐的方式, 所以实际上的周期是两倍Period*/

    uint32_t htim3_period = htim3.Init.Period+1;/* HALL编码器 */
    uint32_t htim6_period = htim6.Init.Period+1;/* 定时器中断 */

    /*定时器频率*/
    uint32_t htim1_freq = 168000000/(htim1.Init.Prescaler+1)/htim1_period;   

    uint32_t htim3_freq = 168000000/(htim3.Init.Prescaler+1)/htim3_period;
    uint32_t htim6_freq = 168000000/(htim6.Init.Prescaler+1)/htim6_period;

    FOC_Motor_Init(&FOC_MOTOR);

    FOC_PWM_Init(&FOC_MOTOR.pwm, htim1_period, FOC_MOTOR.power);

    FOC_HALL_Init(&FOC_MOTOR.hall,  htim3_freq*htim3_period, htim1_freq, FOC_MOTOR.pole_pairs);
    
    FOC_Sensorless_Init(&FOC_MOTOR.sensorless, R/2, L/2, htim1_freq, FOC_MOTOR.pole_pairs);

    PID_Init(&FOC_MOTOR.pid_cur_iq,     htim1_freq);
    PID_Init(&FOC_MOTOR.pid_cur_id,     htim1_freq);
    PID_Init(&FOC_MOTOR.pid_speed,      htim6_freq);
    PID_Init(&FOC_MOTOR.pid_speed_pos,  htim6_freq);
}

void FOC_Start(void)
{
    FOC_Port_Start();
}

void FOC_Stop(void)
{
    FOC_Port_Stop();
}

extern void Foc_Current_Refresh(FOC_MOTOR_t *foc_motor);

/*FOC的运行状态机，PWM定时器->ADC采样->执行此函数*/
void FOC_App_Run(void)
{

    /*更新电流*/
    Foc_Current_Refresh(&FOC_MOTOR);
    
    /*电机状态机运行*/
    FOC_Motor_Run(&FOC_MOTOR);

}








